import os
import launch
import launch_ros
from ament_index_python.packages import get_package_share_directory
from launch.launch_description_sources import PythonLaunchDescriptionSource


def generate_launch_description():
    newrobot_navigation2_dir = get_package_share_directory('myrobot_description')
    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
    rviz_config_dir = os.path.join(nav2_bringup_dir,'rviz','nav2_default_view.rviz')
    use_sim_time = launch.substitutions.LaunchConfiguration(
        'use_sim_time',default='true')
    map_yaml_path = launch.substitutions.LaunchConfiguration(
        'map',default=os.path.join(newrobot_navigation2_dir,'maps','room.yaml')
    )
    nav2_param_path = launch.substitutions.LaunchConfiguration(
        'param_file',default=os.path.join(newrobot_navigation2_dir,'config','nav2_params.yaml')
    )

    return launch.LaunchDescription(
        [
            launch.actions.DeclareLaunchArgument('use_sim_time',default_value=use_sim_time,description='Use Simulation(Gazebo) clock if true'),

            launch.actions.DeclareLaunchArgument('map',default_value=map_yaml_path,description='Full Path to map file to load'),

            launch.actions.DeclareLaunchArgument('params_file',default_value=nav2_param_path,description='Full Path to param file to load'),

            launch.actions.IncludeLaunchDescription(
                PythonLaunchDescriptionSource([nav2_bringup_dir,'/launch','/bringup_launch.py']),
                launch_arguments={
                    'map':map_yaml_path,
                    'use_sim_time':use_sim_time,
                    'params_file':nav2_param_path
                }.items(),
            ),

            launch_ros.actions.Node(
                package='rviz2',
                executable='rviz2',
                name='rviz2',
                arguments=['-d',rviz_config_dir],
                parameters=[{'use_sim_time': use_sim_time}],
                output='screen',
            ),
        ]
    )